/*
 * This file is part of Cleanflight.
 *
 * Cleanflight is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Cleanflight is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#include <rtthread.h>

#ifndef sq
#define sq(x) ((x) * (x)) // 平方
#endif
#define power3(x) ((x) * (x) * (x))

// Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
#define FAST_MATH      // order 9 approximation
#define VERY_FAST_MATH // order 7 approximation

// Use floating point M_PI instead explicitly.
#define M_PIf 3.14159265358979323846f

#define RAD (M_PIf / 180.0f)
#define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
#define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
#define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
#define DEGREES_TO_RADIANS(angle) ((angle)*0.0174532925f)
#define RADIANS_TO_DEGREES(angle) ((angle)*57.29577951f)

#define CM_S_TO_KM_H(centimetersPerSecond) (centimetersPerSecond * 36 / 1000)

#ifndef MIN
#define MIN(a, b) ((a) < (b) ? (a) : (b))
#endif // !MIN
#ifndef MAX
#define MAX(a, b) ((a) > (b) ? (a) : (b))
#endif // !MAX
#ifndef ABS
#define ABS(x) ((x) > 0 ? (x) : -(x))
#endif // !ABS

#define Q12 (1 << 12)

typedef int32_t fix12_t;

typedef struct stdev_s
{
    float m_oldM, m_newM, m_oldS, m_newS;
    int m_n;
} stdev_t;

// Floating point 3 vector.
typedef struct fp_vector
{
    float X;
    float Y;
    float Z;
} t_fp_vector_def;

typedef union u_fp_vector
{
    float A[3];
    t_fp_vector_def V;
} t_fp_vector;

// Floating point Euler angles.
// Be carefull, could be either of degrees or radians.
typedef struct fp_angles
{
    float roll;
    float pitch;
    float yaw;
} fp_angles_def;

typedef union
{
    float raw[3];
    fp_angles_def angles;
} fp_angles_t;
typedef enum
{
    X = 0,
    Y,
    Z,
    W,
} axis_e;

typedef struct
{
    rt_int16_t x;
    rt_int16_t y;
    rt_int16_t z;
} Axis_3_int16_t;
typedef struct
{
    rt_int32_t x;
    rt_int32_t y;
    rt_int32_t z;
} Axis_3_int32_t;
typedef struct
{
    float x;
    float y;
    float z;
} Axis_3_float_t;

int gcd(int num, int denom);
float powerf(float base, int exp);
int32_t applyDeadband(int32_t value, int32_t deadband);
float applyDeadbandFloat(float value, float deadband);
float applyDeadbandFloatNew(float value, float deadband);
void devClear(stdev_t *dev);
void devPush(stdev_t *dev, float x);
float devVariance(stdev_t *dev);
float devStandardDeviation(stdev_t *dev);
float degreesToRadians(int16_t degrees);

int scaleRange(int x, int srcFrom, int srcTo, int destFrom, int destTo);

void normalizeV(struct fp_vector *src, struct fp_vector *dest);

void rotateV(struct fp_vector *v, fp_angles_t *delta);
void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]);

int32_t quickMedianFilter3(int32_t *v);
int32_t quickMedianFilter5(int32_t *v);
int32_t quickMedianFilter7(int32_t *v);
int32_t quickMedianFilter9(int32_t *v);

float quickMedianFilter3f(float *v);
float quickMedianFilter5f(float *v);
float quickMedianFilter7f(float *v);
float quickMedianFilter9f(float *v);

#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
float sin_approx(float x);
float cos_approx(float x);
float atan2_approx(float y, float x);
float acos_approx(float x);
#define tan_approx(x) (sin_approx(x) / cos_approx(x))
#else
#define sin_approx(x) sinf(x)
#define cos_approx(x) cosf(x)
#define atan2_approx(y, x) atan2f(y, x)
#define acos_approx(x) acosf(x)
#define tan_approx(x) tanf(x)
#endif

void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);

int16_t qPercent(fix12_t q);
int16_t qMultiply(fix12_t q, int16_t input);
fix12_t qConstruct(int16_t num, int16_t den);

static inline int constrain(int amt, int low, int high)
{
    if (amt < low)
        return low;
    else if (amt > high)
        return high;
    else
        return amt;
}

inline float constrainf(float amt, float low, float high)
{
    if (amt < low)
        return low;
    else if (amt > high)
        return high;
    else
        return amt;
}
uint16_t crc16_ccitt(uint16_t crc, unsigned char a);
uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length);
uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a);
uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length);